//------------------------------------------------------------------------------
//
//  Hemisson Bumper Service
//
//  Written Jan-2008 by Trevor Taylor, Software Technology, Australia
//
//------------------------------------------------------------------------------

using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using System;
using System.Collections.Generic;
using W3C.Soap;
using dssphttp = Microsoft.Dss.Core.DsspHttp;
// Changed from pxcontactsensor to bumper
using bumper = Microsoft.Robotics.Services.ContactSensor.Proxy;
using xml = System.Xml;


// For [DisplayName] and [Description]
using System.ComponentModel;

using brick = ProMRDS.Robotics.GenericBrick.Proxy;
using submgr = Microsoft.Dss.Services.SubscriptionManager;

using ProMRDS.Robotics.Hemisson;
using util = ProMRDS.Robotics.GenericBrickUtil.Util;

// For HttpGet
using System.Net;
using Microsoft.Dss.Core.DsspHttp;


namespace ProMRDS.Robotics.Hemisson.Bumper
{
    /// <summary>
    /// Hemisson Bumper Service
    /// </summary>
    [Contract(Contract.Identifier)]
    [AlternateContract("http://schemas.microsoft.com/2006/06/contactsensor.html")]
    [DisplayName("Hemisson Generic Contact Sensor")]
    [Description("Provides access to the Hemisson infrared sensors used as a bumpers.\n(Uses Generic Contact Sensors contract.)")]
    public class HemissonBumperService : DsspServiceBase
    {
        /// <summary>
        /// Hemisson Bumper State
        /// </summary>
        [InitialStatePartner(Optional = true)]
        private bumper.ContactSensorArrayState _state = new bumper.ContactSensorArrayState();
        
        /// <summary>
        /// Hemisson Bumper Main Port
        /// </summary>
        /// Changed multiple instances to true
        [ServicePort("/hemissonbumper", AllowMultipleInstances=true)]
        private bumper.ContactSensorArrayOperations _mainPort = new bumper.ContactSensorArrayOperations();

        [EmbeddedResource("ProMRDS.Robotics.Hemisson.Resources.HemissonBumper.xslt")]
        string _transform = null;

        [Partner("Hemisson", Contract = brick.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate, Optional = false)]
        private brick.GenericBrickOperations _brickPort = new brick.GenericBrickOperations();

        [Partner("SubMgr", Contract = submgr.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.CreateAlways, Optional = false)]
        private submgr.SubscriptionManagerPort _subMgrPort = new submgr.SubscriptionManagerPort();

        /// <summary>
        /// Default Service Constructor
        /// </summary>
        public HemissonBumperService(DsspServiceCreationPort creationPort) : 
                base(creationPort)
        {
        }
        
        /// <summary>
        /// Service Start
        /// </summary>
        protected override void Start()
        {
            InitializeState();

            // Subscribe to core Hemisson board for bumper notifications
            SubscribeToHemisson();

            base.Start();
        }

        #region Initialization

        /// <summary>
        /// Initialize State
        /// </summary>
        private void InitializeState()
        {
            bool changed = false;

            // Set up initial state
            if (_state == null)
            {
                _state = new bumper.ContactSensorArrayState();
                changed = true;
            }

            if (_state.Sensors == null)
            {
                _state.Sensors = new List<bumper.ContactSensor>();
                changed = true;
            }
            // Create Sensor array if necessary
            if (_state.Sensors.Count == 0)
            {
                bumper.ContactSensor frontBumper = new bumper.ContactSensor();
                frontBumper.HardwareIdentifier = (int)HwIds.IRFront;
                frontBumper.Name = "Front InfraRed Bumper";
                _state.Sensors.Add(frontBumper);

                bumper.ContactSensor frontRightBumper = new bumper.ContactSensor();
                frontRightBumper.HardwareIdentifier = (int)HwIds.IRFrontRight;
                frontRightBumper.Name = "Front Right InfraRed Bumper";
                _state.Sensors.Add(frontRightBumper);

                bumper.ContactSensor frontLeftBumper = new bumper.ContactSensor();
                frontLeftBumper.HardwareIdentifier = (int)HwIds.IRFrontLeft;
                frontLeftBumper.Name = "Front Left InfraRed Bumper";
                _state.Sensors.Add(frontLeftBumper);

                bumper.ContactSensor rightBumper = new bumper.ContactSensor();
                rightBumper.HardwareIdentifier = (int)HwIds.IRRight;
                rightBumper.Name = "Right InfraRed Bumper";
                _state.Sensors.Add(rightBumper);

                bumper.ContactSensor leftBumper = new bumper.ContactSensor();
                leftBumper.HardwareIdentifier = (int)HwIds.IRLeft;
                leftBumper.Name = "Left InfraRed Bumper";
                _state.Sensors.Add(leftBumper);

                bumper.ContactSensor rearBumper = new bumper.ContactSensor();
                rearBumper.HardwareIdentifier = (int)HwIds.IRRear;
                rearBumper.Name = "Rear InfraRed Bumper";
                _state.Sensors.Add(rearBumper);

                bumper.ContactSensor groundRight = new bumper.ContactSensor();
                groundRight.HardwareIdentifier = (int)HwIds.IRGroundRight;
                groundRight.Name = "Ground Right InfraRed Bumper";
                _state.Sensors.Add(groundRight);

                bumper.ContactSensor groundLeft = new bumper.ContactSensor();
                groundLeft.HardwareIdentifier = (int)HwIds.IRGroundLeft;
                groundLeft.Name = "Ground Left InfraRed Bumper";
                _state.Sensors.Add(groundLeft);

                changed = true;
            }

            if (changed)
                SaveState(_state);

        }


        /// <summary>
        /// Subscribe to the Hemisson robot "brick"
        /// </summary>
        private void SubscribeToHemisson()
        {
            // Create a notification port
            brick.GenericBrickOperations notificationPort = new brick.GenericBrickOperations();

            // Subscribe to the basic stamp and wait for a response
            Activate(
                Arbiter.Choice(_brickPort.Subscribe(notificationPort),
                    delegate(SubscribeResponseType Rsp)
                    {
                        // Note: To manage concurrency correctly between our main handlers and
                        // notifications we combine the main interleave arbiter protecting our handlers
                        // with notification from the infra red device
                        base.MainPortInterleave.CombineWith(new Interleave(
                            new ExclusiveReceiverGroup(
                                Arbiter.Receive<brick.UpdateSensors>(true, notificationPort, UpdateSensorsHandler)
                            ), new ConcurrentReceiverGroup()
                        ));
                    },
                    delegate(Fault fault)
                    {
                        LogError("Subscription failed", fault);
                    }
                )
            );

        }

        #endregion

        #region Operation Handlers

        /// <summary>
        /// Get Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetHandler(bumper.Get get)
        {
            get.ResponsePort.Post(_state);
            yield break;
        }
        
        /// <summary>
        /// HttpGet Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> HttpGetHandler(HttpGet get)
        {
            get.ResponsePort.Post(new HttpResponseType(
                HttpStatusCode.OK,
                _state,
                _transform)
            );
            yield break;
        }
        
        /// <summary>
        /// Replace Handler
        /// </summary>
        /// <param name="replace"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> ReplaceHandler(bumper.Replace replace)
        {
            _state = replace.Body;
            replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
            yield break;
        }
        
        /// <summary>
        /// Update Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> UpdateHandler(bumper.Update update)
        {
            throw new NotImplementedException("Bumper Sensors are not Updateable");
        }
        
        /// <summary>
        /// ReliableSubscribe Handler
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> ReliableSubscribeHandler(bumper.ReliableSubscribe subscribe)
        {
            base.SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort);
            yield break;
        }
        
        /// <summary>
        /// Subscribe Handler
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> SubscribeHandler(bumper.Subscribe subscribe)
        {
            base.SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort);
            yield break;
        }

        #endregion

        #region Notification Handlers

        // IMPORTANT NOTE:
        // These routines determine whether a bumper indicator is true or false.
        // The values in here are critical to the operation of the virtual bumpers.

        // Hemisson IR sensors are EXTREMELY short range
        // A value of 10 corresponds to about 3cm and closer than that it
        // increases rapidly
        private bool BumperPressed(int value)
        {
            if (value >= 10)
                return true;
            else
                return false;
        }

        // Hemisson line detectors are not very sensitive and they are also
        // not balanced, i.e. the two detectors behave differently!
        // A value of 100 was found by trial and error, but it will not
        // work reliably on all types of paper and ink.
        // Notice that dark lines produce lower values.
        private bool LineDetected(int value)
        {
            if (value <= 100)
                return true;
            else
                return false;
        }

        /// <summary>
        /// Receive infrared notifications from the Hemisson service
        /// </summary>
        /// <param name="notification"></param>
        private void UpdateSensorsHandler(brick.UpdateSensors notification)
        {
            int i;
            bool changed;
            bool found;
            bool newState;

            foreach (bumper.ContactSensor b in _state.Sensors)
            {
                changed = false;
                found = false;
                foreach (brick.Device dev in notification.Body.Devices)
                {
                    if (dev.HardwareIdentifer == b.HardwareIdentifier)
                    {
                        found = true;
                        // Check on the purpose of the device
                        // NOTE: IR sensors are NOT marked as Bumpers!
                        // A Bumper is a physical switch, or an IR sensor that only
                        // has a Digital output. The Hemisson IR sensors are range
                        // sensors, except for the Line Followers.
                        if (dev.Function == brick.DeviceFunctions.LineFollower)
                            newState = LineDetected((int)dev.Value);
                        else
                            newState = BumperPressed((int)dev.Value);
                        if (newState != b.Pressed)
                        {
                            changed = true;
                            b.Pressed = newState;
                            b.TimeStamp = DateTime.Now;
                            break;
                        }
                    }
                }

                if (!found)
                    LogError(LogGroups.Console, "IR sensor not found in update");

                if (changed)
                    this.SendNotification<bumper.Update>(_subMgrPort, new bumper.Update(b));
            }

            /*
            foreach (bumper.ContactSensor b in _state.Sensors)
            {
                bool changed = false;

                if (b.HardwareIdentifier == (int)hemi.IRSensors.FrontLeft && b.Pressed != BumperPressed(notification.Body.IRFrontLeft))
                {
                    b.Pressed = BumperPressed(notification.Body.IRFrontLeft);
                    changed = true;
                }


                else
                {
                    Console.WriteLine("Invalid IR Hardware Identifier");
                }

                if (changed)
                    this.SendNotification<bumper.Update>(_subMgrPort, new bumper.Update(b));

            }
             */

        }

        // This is one approach ... a better way is to use the device function
        private bool IsLineFollower(brick.Device d)
        {
            if (d.Location == brick.Location.GroundFrontCenter ||
                d.Location == brick.Location.GroundFrontFarLeft ||
                d.Location == brick.Location.GroundFrontFarRight ||
                d.Location == brick.Location.GroundFrontLeft ||
                d.Location == brick.Location.GroundFrontRight)
                return true;
            else
                return false;
        }

        #endregion

    }
}
